//
// Created by Administrator on 2019\10\24 0024.
//

#include "TCLStereoDepthAlg.h"

#include "iostream"
#include "tcl_dualcam_def.h"

#include <regex>

#define DIST_PARAM_NUM 8

struct CalibrationFile {
	int phone_id;
	int full_size_w;
	int full_size_h;
	int crop_size_w;
	int crop_size_h;
	int orientation;
	int size_w;
	int size_h;
	float MIN_DISP;
	float MAX_DISP;
	float delta_disp_per_degree;
	double M1[3][3];
	double D1[1][DIST_PARAM_NUM];
	double M2[3][3];
	double D2[1][DIST_PARAM_NUM];
	double R[3][3];
	double T[3][1];
};


bool GetSortedImagePathPairs(
        std::vector<cv::String> & files_aux,
        std::vector<cv::String> & files_main,
        const cv::String & input_dir
){
    std::vector<cv::String> files_aux_unsorted;
    std::vector<cv::String> files_main_unsorted;

    cv::glob(input_dir + "/picAux/", files_aux_unsorted);
    cv::glob(input_dir + "/picMain/", files_main_unsorted);

    if(files_aux_unsorted.size() != files_main_unsorted.size()){
        VIDEO_DEPTH_LOGE("Input aux files not equal to input main files!");
        return false;
    }

	VIDEO_DEPTH_LOGI("number of images: %d", files_main_unsorted.size());

    std::vector<std::pair<int, int>> pair_aux;
    std::vector<std::pair<int, int>> pair_main;

    std::stringstream ss("");
    int idx;
	int aux_prefix_len = (input_dir + "/picAux/").size();
	int main_prefix_len = (input_dir + "/picMain/").size();
	std::regex reg_aux("_(\\d+)A");
	std::regex reg_main("_(\\d+)M");

    for (int i = 0; i < files_aux_unsorted.size(); ++i) {
		std::match_results<std::string::iterator> matched_str;
		std::string target_str = files_aux_unsorted[i].substr(aux_prefix_len, -1);
		bool found = std::regex_search(target_str.begin(), target_str.end(), matched_str, reg_aux);
		if (!found) {
			VIDEO_DEPTH_LOGE("failed to interpretate the filename: [%s]!", target_str);
			PAUSE();
			FAIL_EXIT(-1);
		}
		int aux_id = atoi(matched_str[1].str().c_str());
		
		matched_str = std::match_results<std::string::iterator>();
		target_str = files_main_unsorted[i].substr(main_prefix_len, -1);
		found = std::regex_search(target_str.begin(), target_str.end(), matched_str, reg_main);
		if (!found) {
			VIDEO_DEPTH_LOGE("failed to interpretate the filename: [%s]!", target_str);
			PAUSE();
			FAIL_EXIT(-1);
		}
		int main_id = atoi(matched_str[1].str().c_str());
		
		VIDEO_DEPTH_LOGI("A-M: %d-%d", aux_id, main_id);
		//PAUSE();

		pair_aux.push_back(std::pair<int, int>(aux_id, i));
        pair_main.push_back(std::pair<int, int>(main_id, i));
    }

    std::sort(pair_aux.begin(), pair_aux.end());
    std::sort(pair_main.begin(), pair_main.end());

    for (int i = 0; i < pair_aux.size(); ++i) {
        files_aux.push_back(files_aux_unsorted[pair_aux[i].second]);
        files_main.push_back(files_main_unsorted[pair_main[i].second]);
    }
    return true;
}


int main(int argc, const char ** argv){
	cv::String base_dir = "F:/dump/TEST-ALL/Dataset/video/";
	//cv::String date_id = "20200908";
	//cv::String phone_id = "oakland0068";
	//cv::String date_id = "20200908";
	//cv::String phone_id = "ak57";
	//cv::String date_id = "20201210";
	//cv::String phone_id = "HongKongPro0077";
	cv::String date_id = "20210203";
	cv::String phone_id = "T1Pro0115";

	cv::String ori_dir = "ori/";
	cv::String cal_dir = "cal/";
	cv::String depth_dir = "depth/";
	cv::String dump_dir = "dump/";
	String cal_ver = "version8";

    double beg_, end_;
    double freq = cv::getTickFrequency();
	tcl_depth_video::TCLStereoDepthAlg_prv *m_DepthAlgorithmPtr = new tcl_depth_video::TCLStereoDepthAlg_prv;
	tcl_depth_video::Param_DepthPreview param_depth;

	// calibration settings
	int orientation;
	cv::Size cali_size;
	cv::Size full_size;
	cv::Size crop_size;
	cv::Mat M1, D1, M2, D2;
	//float min_disp, delta_disp_per_degree;

#define USE_CALI_BIN
#ifdef USE_CALI_BIN
	String cali_bin = base_dir + "/" + cal_dir + "/" + phone_id + "/" + cal_ver + "/dualcam_cali.bin";
	std::fstream ifs(cali_bin, std::ios::in | std::ios::binary);
	CalibrationFile califile_in;
	if (!ifs.is_open()) {
		VIDEO_DEPTH_LOGE("dualcam_cali.bin not found!");
		return -1;
	}
	ifs.read((char*)&califile_in, sizeof(califile_in));

	orientation = califile_in.orientation;
	cali_size.width = califile_in.size_w;
	cali_size.height = califile_in.size_h;
	full_size.width = califile_in.full_size_w;
	full_size.height = califile_in.full_size_h;
	crop_size.width = califile_in.crop_size_w;
	crop_size.height = califile_in.crop_size_h;
	M1 = cv::Mat(Size(3, 3), CV_64FC1, califile_in.M1);
	D1 = cv::Mat(Size(1, DIST_PARAM_NUM), CV_64FC1, califile_in.D1);
	M2 = cv::Mat(Size(3, 3), CV_64FC1, califile_in.M2);
	D2 = cv::Mat(Size(1, DIST_PARAM_NUM), CV_64FC1, califile_in.D2);
#else
	String intrinsic = base_dir + "/" + cal_dir + "/" + phone_id + "/" + cal_ver + "/stereo_intrinsics.yml";
	FileStorage fs(intrinsic, FileStorage::READ);
	if (!fs.isOpened()) {
		VIDEO_DEPTH_LOGE("Failed to open file %s\n", intrinsic.c_str());
		return -1;
	}

	fs["orientation"] >> orientation;
	fs["size_w"] >> cali_size.width;
	fs["size_h"] >> cali_size.height;
	fs["full_size_w"] >> full_size.width;
	fs["full_size_h"] >> full_size.height;
	fs["crop_size_w"] >> crop_size.width;
	fs["crop_size_h"] >> crop_size.height;
	fs["M1"] >> M1;
	fs["D1"] >> D1;
	fs["M2"] >> M2;
	fs["D2"] >> D2;
#endif

#define P_VENICE 0
#define P_OAKLAND 1
#define P_AK57 2
#define P_HONGKONG 3
#define P_T1PRO 4

#define PRODUCT_ID P_T1PRO
	
	///////////////// Calibration Data Settings //////////////////
	param_depth.orientation = orientation;
	param_depth.cali_size = cali_size;
	param_depth.full_size = full_size;
	param_depth.crop_size = crop_size;
	param_depth.intrinsics.M1 = M1;
	param_depth.intrinsics.D1 = D1; /////////// to be removed if distortion issue is fixed
	param_depth.intrinsics.M2 = M2;
	param_depth.intrinsics.D2 = D2;
	//param_depth.min_disp = min_disp;

	////////// Constants Depending on Phone Series ///////////////
#if PRODUCT_ID==P_VENICE
	// venice settings
	param_depth.full_size = cv::Size(4608, 3456);
	param_depth.crop_size = cv::Size(3264, 2448);
	param_depth.orientation = TCL_IMAGE_ROTATE_NONE;
#elif PRODUCT_ID==P_OAKLAND	
	// oakland settings
	param_depth.full_size = cv::Size(4608, 3456);
	param_depth.crop_size = cv::Size(4377, 3283);
	param_depth.orientation = TCL_IMAGE_ROTATE_COUNTER_CLOCKWISE_90;
#endif

	// algorithm settings
    param_depth.depth_size = cv::Size(384, 288);
    param_depth.dump_enabled = true;
    param_depth.dump_path = base_dir + dump_dir + date_id + "/" + phone_id;

    m_DepthAlgorithmPtr->Init(param_depth);

    cv::String input_dir = base_dir + ori_dir + date_id + "/" + phone_id;
    std::vector<cv::String> files_aux, files_main;
    GetSortedImagePathPairs(files_aux, files_main, input_dir);

    // retrieve focus coordinates
    // DEMO URI in NV21 format: 181_Main2-Preview_-2122246576_960x720_doflevel80_focus467_349.nv21
    const char * beg_mark = "focus";
    const char * end_mark = ".nv21";
	//float yuv_width = 960;
	//float yuv_height = 720;
	float yuv_width = 1280;
	float yuv_height = 960;

    //std::vector<cv::Point2f> focuses;
    //focuses.resize(files_aux.size());

    //for (int i=0; i<files_aux.size(); ++i) {
    //    size_t idx_b = files_aux[i].find(beg_mark);
    //    size_t idx_e = files_aux[i].find(end_mark);
    //    cv::String focus_str = files_aux[i].substr(idx_b + strlen(beg_mark),
    //                                                idx_e - idx_b - strlen(beg_mark));
    //    size_t idx_u = focus_str.find('_');
    //    float f_x = atoi(focus_str.substr(0, idx_u).c_str());
    //    float f_y = atoi(focus_str.substr(idx_u+1, -1).c_str());
    //    focuses[i] = (cv::Point2f(f_x / yuv_width, f_y / yuv_height));
    //    VIDEO_DEPTH_LOGI("Focus point: (%.4f, %.4f)", focuses[i].x, focuses[i].y);
    //}

    // create a global uid for synchronize log/dump information
    cv::String uid;
	cv::String output_dir = base_dir + "/depth/" + date_id + "/" + phone_id + "/";
    VIDEO_DEPTH_LOGI("NUMBER OF FILES FOUND: %lu.", files_main.size());
	//PAUSE();
	
    LPTCL_IMAGE yuv_aux = tcl_depth_video::create_yuv_image(yuv_width, yuv_height, yuv_width, TCL_PAF_NV12);
    LPTCL_IMAGE yuv_main = tcl_depth_video::create_yuv_image(yuv_width, yuv_height, yuv_width, TCL_PAF_NV12);

    for (int i=0; i<files_main.size(); ++i) {
		tcl_depth_video::load_yuv_data(yuv_aux, files_aux[i].c_str());
		tcl_depth_video::load_yuv_data(yuv_main, files_main[i].c_str());
        int flag = 1;

        beg_ = cv::getTickCount();
		cv::Mat depth(param_depth.depth_size, CV_8UC1);

        if (i == 0) uid = cv::String("INIT");
        else {
#ifdef _WIN32
			size_t fn_idx = files_main[i - 1].find_last_of('\\');
#else
            size_t fn_idx = files_main[i-1].find_last_of('/');
#endif
            uid = files_main[i-1].substr(fn_idx + 1, files_main[i-1].size()- 6 - fn_idx);
        }
        VIDEO_DEPTH_LOGI("filename: %s", uid.c_str());

        float dist = m_DepthAlgorithmPtr->Compute(depth, yuv_aux, yuv_main, uid, 0, 0, flag);

        end_ = cv::getTickCount();
        VIDEO_DEPTH_LOGI("Depth Computation Cost: %.4f ms.", (1000 * (end_ - beg_) / freq));
        VIDEO_DEPTH_LOGI("focus: %6.0f mm.", dist);

		if (dist <= 0) {
			VIDEO_DEPTH_LOGE("=============FAIL TO GET DEPTH MAP!==============");
			//PAUSE();
			//FAIL_EXIT(-1);
		} {
			tcl_depth_video::DumpImage(depth, output_dir, "fine_depth", uid);
			this_thread::sleep_for(std::chrono::milliseconds(1000));
			//PAUSE();
		}
        if ( i==files_main.size()-1 ) {
            size_t fn_idx = files_main[i].find_last_of('/');
            uid = files_main[i].substr(fn_idx + 1, files_main[i].size()- 6 - fn_idx);
            dist = m_DepthAlgorithmPtr->Compute(depth, yuv_aux, yuv_main, uid, 0, 0, flag);
			if (dist <= 0) {
				VIDEO_DEPTH_LOGE("=============FAIL TO GET DEPTH MAP!==============");
				//PAUSE();
				//FAIL_EXIT(-1);
			} {
				tcl_depth_video::DumpImage(depth, output_dir, "fine_depth", uid); 
			}
        }
    }

	tcl_depth_video::free_yuv_image(yuv_aux);
	tcl_depth_video::free_yuv_image(yuv_main);
    delete(m_DepthAlgorithmPtr);

	PAUSE();
	return 0;
}